/////////////////////////////////////////////////////////////////////////
//
// Amuse Engine SDK - core/math
// Copyright (c) 2014.  All Rights Reserved
//
// File:		AEVector3.h
// Author:		Gianluca Belardelli
// Date:		29/07/2013
// Purpose:		Implementazione dei metodi inline della classe AEVector3
//
// Revision:
//				001 - 29/07/2013
//					+ Revisione iniziale
//
/////////////////////////////////////////////////////////////////////////
#ifndef _AEVECTOR3_INL_
#define _AEVECTOR3_INL_

#include <core/math/vector/AEVector2.h>
#include <core/math/vector/AEVector4f.h>

// Helper macro to copy vector parameters into local variables
// This might look like stupid unnecessary code, but it helps the compiler to optimize the assembly code
#define AEVECTOR3_COPYTEMPS(var) \
  const float var##_x = var.fX; const float var##_y = var.fY; const float var##_z = var.fZ;

AE_FORCEINLINE AEVector3::AEVector3( void ) { }
AE_FORCEINLINE AEVector3::AEVector3( float _fX, float _fY, float _fZ ) : fX ( _fX ), fY ( _fY ), fZ ( _fZ ) { }
AE_FORCEINLINE AEVector3::AEVector3( float fXYZ ) : fX ( fXYZ ), fY ( fXYZ ), fZ ( fXYZ ) { }

AE_FORCEINLINE void AEVector3::SetZero( void )
{
	fX = 0.0f;
	fY = 0.0f;
	fZ = 0.0f;
}

AE_FORCEINLINE void AEVector3::Set( float _fX, float _fY, float _fZ )
{
	fX = _fX;
	fY = _fY;
	fZ = _fZ;
}

AE_FORCEINLINE void AEVector3::Set( float fXYZ )
{
	fX = fXYZ;
	fY = fXYZ;
	fZ = fXYZ;
}

AE_FORCEINLINE float AEVector3::operator[] ( AEUINT32 uiIdx ) const
{
	AEASSERT( uiIdx <= 2 );

	return fData[uiIdx];
}

AE_FORCEINLINE float& AEVector3::operator[] ( AEUINT32 uiIdx )
{
	AEASSERT( uiIdx <= 2 );

	return fData[uiIdx];
}

AE_FORCEINLINE const AEVector3 AEVector3::operator- ( void ) const
{
	return AEVector3( -fX, -fY, -fZ );
}

AE_FORCEINLINE void AEVector3::operator+= ( const AEVector3 &vcRhs )
{
	AEVECTOR3_COPYTEMPS( vcRhs );
	fX += vcRhs_x;
	fY += vcRhs_y;
	fZ += vcRhs_z;
}

AE_FORCEINLINE void AEVector3::operator-= (const AEVector3 &vcRhs )
{
	AEVECTOR3_COPYTEMPS( vcRhs );
	fX -= vcRhs_x;
	fY -= vcRhs_y;
	fZ -= vcRhs_z;
}

AE_FORCEINLINE void AEVector3::operator+= ( float fValue )
{
	fX += fValue;
	fY += fValue;
	fZ += fValue;
}

AE_FORCEINLINE void AEVector3::operator-= ( float fValue )
{
	fX -= fValue;
	fY -= fValue;
	fZ -= fValue;
}

AE_FORCEINLINE void AEVector3::operator*= ( float fValue )
{
	fX *= fValue;
	fY *= fValue;
	fZ *= fValue;
}

AE_FORCEINLINE void AEVector3::operator/= ( float fValue )
{
	const float fRes = 1.0f / fValue;

	fX *= fRes;
	fY *= fRes;
	fZ *= fRes;
}

AE_FORCEINLINE bool AEVector3::IsZero( float fEpsilon ) const
{
	AEASSERT( fEpsilon >= 0.0f );

	if( fEpsilon == 0.0f )
		return ( ( fX == 0 ) && ( fY == 0 ) && ( fZ == 0 ) );

	return ( ( fX >= -fEpsilon ) && ( fX <= fEpsilon ) &&
			 ( fY >= -fEpsilon ) && ( fY <= fEpsilon ) &&
			 ( fZ >= -fEpsilon ) && ( fZ <= fEpsilon ) );
}

AE_FORCEINLINE bool AEVector3::IsValid( void ) const
{
	return ( AEMath::IsFiniteNumber( fX ) && AEMath::IsFiniteNumber( fY ) && AEMath::IsFiniteNumber( fZ ) );
}

AE_FORCEINLINE bool AEVector3::IsNormalized( float fEpsilon ) const
{
	AEASSERT( fEpsilon >= 0.0f );

	const float fLen = GetLengthSquared();

	return AEMath::IsFloatEqual( fLen, 1.0f, fEpsilon );
}

AE_FORCEINLINE bool AEVector3::IsIdentical( const AEVector3 &vcRhs ) const
{
	AEVECTOR3_COPYTEMPS( vcRhs );
	return ( ( fX == vcRhs_x ) && ( fY == vcRhs_y ) && ( fZ == vcRhs_z ) );
}

AE_FORCEINLINE bool AEVector3::IsEqual( const AEVector3 &vcRhs, float fEpsilon ) const
{
	AEASSERT( fEpsilon >= 0.0f );

	AEVECTOR3_COPYTEMPS( vcRhs );

	return ( AEMath::IsFloatEqual( fX, vcRhs_x, fEpsilon ) &&
			 AEMath::IsFloatEqual( fY, vcRhs_y, fEpsilon ) &&
			 AEMath::IsFloatEqual( fZ, vcRhs_z, fEpsilon ) );
}

AE_FORCEINLINE float AEVector3::GetLengthSquared( void ) const
{
	return ( fX * fX + fY * fY + fZ * fZ );
}

AE_FORCEINLINE float AEVector3::GetLength( void ) const
{
	return AEMath::Sqrt( GetLengthSquared() );
}

AE_FORCEINLINE float AEVector3::GetLengthAndNormalize( void )
{
	const float fLength = GetLength();
	operator/= ( fLength );

	return fLength;
}

AE_FORCEINLINE float AEVector3::GetLengthInverse( void ) const
{
	return AEMath::SqrtInverse( GetLengthSquared() );
}

AE_FORCEINLINE const AEVector3 AEVector3::GetNormalized( void ) const
{
	AEVector3 vcTemp( *this );
	vcTemp.Normalize();
	return vcTemp;
}

AE_FORCEINLINE void AEVector3::Normalize( void )
{
  operator*= ( GetLengthInverse() );
}

AE_FORCEINLINE AERESULT AEVector3::NormalizeIfNotZero( float fEpsilon )
{
	if( ( IsZero( fEpsilon ) ) || ( !IsValid() ) )
		return AE_FAILURE;

	Normalize();
	return AE_SUCCESS;
}

AE_FORCEINLINE float AEVector3::NormalizedEnsureUnitLength( void )
{
	if( ( IsZero( AEMATH_EPSILON ) ) || ( !IsValid() ) )
	{
		Set( 1, 0, 0 );
		return 0.0f;
	}

	const float fLen = GetLength();
	*this /= fLen;

	return fLen;
}

AE_FORCEINLINE float AEVector3::GetDistanceTo( const AEVector3 &vcRhs ) const
{
	return ( vcRhs - *this ).GetLength();
}

AE_FORCEINLINE float AEVector3::GetDistanceToSquared( const AEVector3 &vcRhs ) const
{
	return ( vcRhs - *this ).GetLengthSquared();
}

AE_FORCEINLINE AERESULT AEVector3::SetLength( float fLen, float fEpsilon )
{
	const AERESULT aeRes = NormalizeIfNotZero( fEpsilon );
	operator*= ( fLen );

	return aeRes;
}

AE_FORCEINLINE void AEVector3::SetMin( const AEVector3 &vcRhs )
{
	SetMin( *this, vcRhs );
}

AE_FORCEINLINE void AEVector3::SetMax( const AEVector3 &vcRhs )
{
	SetMax( *this, vcRhs );
}

AE_FORCEINLINE void AEVector3::ClampTo( const AEVector3 &vcMinVal, const AEVector3 &vcMaxVal )
{
	SetClamped( *this, vcMinVal, vcMaxVal );
}

AE_FORCEINLINE const AEVector3 AEVector3::CompMul( const AEVector3 &vcRhs ) const
{
	AEVECTOR3_COPYTEMPS( vcRhs );

	return AEVector3( fX * vcRhs_x, fY * vcRhs_y, fZ * vcRhs_z );
}

AE_FORCEINLINE const AEVector3 AEVector3::CompDiv( const AEVector3 &vcRhs ) const
{
	AEVECTOR3_COPYTEMPS( vcRhs );

	return AEVector3( fX / vcRhs_x, fY / vcRhs_y, fZ / vcRhs_z );
}

AE_FORCEINLINE void AEVector3::SetMin( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	AEVECTOR3_COPYTEMPS( vcLhs );
	AEVECTOR3_COPYTEMPS( vcRhs );

	fX = AEMath::Min( vcLhs_x, vcRhs_x );
	fY = AEMath::Min( vcLhs_y, vcRhs_y );
	fZ = AEMath::Min( vcLhs_z, vcRhs_z );
}

AE_FORCEINLINE void AEVector3::SetMax( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	AEVECTOR3_COPYTEMPS( vcLhs );
	AEVECTOR3_COPYTEMPS( vcRhs );

	fX = AEMath::Max( vcLhs_x, vcRhs_x );
	fY = AEMath::Max( vcLhs_y, vcRhs_y );
	fZ = AEMath::Max( vcLhs_z, vcRhs_z );
}

AE_FORCEINLINE void AEVector3::SetClamped( const AEVector3 &vcVal, const AEVector3 &vcMinVal, const AEVector3 &vcMaxVal )
{
	AEVECTOR3_COPYTEMPS( vcVal );
	AEVECTOR3_COPYTEMPS( vcMinVal );
	AEVECTOR3_COPYTEMPS( vcMaxVal );

	fX = AEMath::Clamp( vcVal_x, vcMinVal_x, vcMaxVal_x );
	fY = AEMath::Clamp( vcVal_y, vcMinVal_y, vcMaxVal_y );
	fZ = AEMath::Clamp( vcVal_z, vcMinVal_z, vcMaxVal_z );
}

AE_FORCEINLINE void AEVector3::SetAbs( const AEVector3 &vcVal )
{
	AEVECTOR3_COPYTEMPS( vcVal );

	fX = AEMath::Abs( vcVal_x );
	fY = AEMath::Abs( vcVal_y );
	fZ = AEMath::Abs( vcVal_z );
}

AE_FORCEINLINE void AEVector3::SetCross( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	*this = vcLhs.Cross( vcRhs );
}

AE_FORCEINLINE void AEVector3::SetInterpolate( const AEVector3 &vcLhs, const AEVector3 &vcRhs, float fFactor )
{
	AEVECTOR3_COPYTEMPS( vcLhs );
	AEVECTOR3_COPYTEMPS( vcRhs );

	fX = AEMath::Interpolate( vcLhs_x, vcRhs_x, fFactor );
	fY = AEMath::Interpolate( vcLhs_y, vcRhs_y, fFactor );
	fZ = AEMath::Interpolate( vcLhs_z, vcRhs_z, fFactor );
}

AE_FORCEINLINE float AEVector3::Dot( const AEVector3 &vcRhs ) const
{
	AEVECTOR3_COPYTEMPS( vcRhs );

	return ( ( fX * vcRhs_x ) + ( fY * vcRhs_y ) + ( fZ * vcRhs_z ) );
}

AE_FORCEINLINE const AEVector3 AEVector3::Cross( const AEVector3 &vcRhs ) const
{
	AEVECTOR3_COPYTEMPS( vcRhs );

	const float fResX = fY * vcRhs_z - fZ * vcRhs_y;
	const float fResY = fZ * vcRhs_x - fX * vcRhs_z;
	const float fResZ = fX * vcRhs_y - fY * vcRhs_x;

	return AEVector3( fResX, fResY, fResZ );
}

AE_FORCEINLINE void AEVector3::MakeOrthogonalTo( const AEVector3 &vcNormal )
{
	//HKVMATH_ASSERT( vcNormal.IsNormalized(), "The normal must be normalized." );
	AEASSERT( vcNormal.IsNormalized() );
	// get the distance v is along this vector
	const float fDot = this->Dot( vcNormal );

	//subtract the distance to make it orthogonal
	*this -= fDot * vcNormal;
}

AE_FORCEINLINE const AEVector3 AEVector3::GetOrthogonalVector( void ) const
{
	const AEVector3 vcTemp = GetNormalized();

	if( AEMath::Abs( vcTemp.Dot( AEVector3( 1, 0, 0 ) ) ) > 0.8f )
		return vcTemp.Cross( AEVector3( 0, 1, 0 ) );

	return vcTemp.Cross( AEVector3( 1, 0, 0 ) );
}

AE_FORCEINLINE void AEVector3::Negate( void )
{
	fX = -fX;
	fY = -fY;
	fZ = -fZ;
}

AE_FORCEINLINE void AEVector3::Reflect( const AEVector3 &vcNormal )
{
	*this = GetReflected( vcNormal );
}

AE_FORCEINLINE const AEVector3 AEVector3::GetReflected( const AEVector3 &vcNormal ) const
{
	//HKVMATH_ASSERT( vcNormal.IsNormalized(), "The normal must be normalized." );
	AEASSERT( vcNormal.IsNormalized() );

	const float fTemp = this->Dot( vcNormal );

	return ( *this ) - 2.0f * fTemp * vcNormal;
}

AE_FORCEINLINE const AEVector4f AEVector3::GetAsPosition( void ) const
{
	return AEVector4f( fX, fY, fZ, 1.0f );
}

AE_FORCEINLINE const AEVector4f AEVector3::GetAsDirection( void ) const
{
	return AEVector4f( fX, fY, fZ, 0.0f );
}

AE_FORCEINLINE const AEVector2 AEVector3::GetAsVec2( void ) const
{
	return AEVector2( fX, fY );
}

AE_FORCEINLINE const AEVector4f AEVector3::GetAsVec4( float fW ) const
{
	return AEVector4f( fX, fY, fZ, fW );
}

AE_FORCEINLINE bool operator== ( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	return vcLhs.IsIdentical( vcRhs );
}

AE_FORCEINLINE bool operator!= ( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	return !vcLhs.IsIdentical( vcRhs );
}

AE_FORCEINLINE const AEVector3 operator+ ( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	AEVector3 vcTemp( vcLhs );
	vcTemp += vcRhs;
	return vcTemp;
}

AE_FORCEINLINE const AEVector3 operator- ( const AEVector3 &vcLhs, const AEVector3 &vcRhs )
{
	AEVector3 vcTemp( vcLhs );
	vcTemp -= vcRhs;
	return vcTemp;
}

AE_FORCEINLINE const AEVector3 operator* ( const AEVector3 &vcLhs, float fValue )
{
	AEVector3 vcTemp( vcLhs );
	vcTemp *= fValue;
	return vcTemp;
}

AE_FORCEINLINE const AEVector3 operator* ( float fValue, const AEVector3 &vcRhs )
{
	AEVector3 vcTemp( vcRhs );
	vcTemp *= fValue;
	return vcTemp;
}

AE_FORCEINLINE const AEVector3 operator/ ( const AEVector3 &vcLhs, float fValue )
{
	AEVector3 vcTemp( vcLhs );
	vcTemp /= fValue;
	return vcTemp;
}

AE_FORCEINLINE float AEVector3::GetAngleBetweenDeg( const AEVector3 &vcRhs ) const
{
	const AEVector3 vcV1 = GetNormalized();
	const AEVector3 vcV2 = vcRhs.GetNormalized();

	float fDot = AEMath::Clamp( vcV1.Dot( vcV2 ), -1.0f, 1.0f ); // to ensure that no precision loss yields values outside this range
	return AEMath::Acos( fDot );
}

AE_FORCEINLINE float AEVector3::GetAngleBetweenRad( const AEVector3 &vcRhs ) const
{
	const AEVector3 vcV1 = GetNormalized();
	const AEVector3 vcV2 = vcRhs.GetNormalized();

	float fDot = AEMath::Clamp( vcV1.Dot( vcV2 ), -1.0f, 1.0f ); // to ensure that no precision loss yields values outside this range
	return AEMath::Acos( fDot );
}

#endif
// _AEVECTOR3_INL_
